

#include <string>

#include "LEOAttitude.hpp"
#include "ReferenceSystem.hpp"
#include "CommonTime.hpp"

using namespace std;

#define debug 1

namespace gpstk
{
   void LEOAttitude::loadFile( std::string filename )
   {
      ifstream input(filename.c_str());
      if (!input)
      {
         FileMissingException 
            fe ( "Cannot Open  LEOAttitude file: " + filename );
         GPSTK_THROW(fe);
      }

      bool ok(true);
      bool firstTime(true);

      string line;

      while( !input.eof() && input.good() )
      {
         if( input.eof() ) break;
         if( input.bad() ) { ok = false; break; }

         getline( input, line ); 
         istringstream iss(line);

         int year, doy;
         double sec, q0, q1, q2, q3;
         
         iss >> year >> doy >> sec >> q0 >> q1 >> q2 >> q3 ;
         
         std::vector<double> QVec;
         QVec.push_back(q0);
         QVec.push_back(q1);
         QVec.push_back(q2);
         QVec.push_back(q3);


         YDSTime yds(year, doy, sec);
         CommonTime epoch = yds.convertToCommonTime();
         
         QuaterMap[epoch] = QVec;
         
         QVec.clear();

      }

      input.close();

      if( !ok )
      {
         FileMissingException fe( filename + "is corrupted..." );
         GPSTK_THROW(fe);
      }


   } // END of "void loadLEOAttitudeFile"



   Matrix<double> LEOAttitude::S2TMatrix(CommonTime& epoch)
   {
         // The corresponding second in N half.

      Matrix<double> rotMatrix;

      int N(9);
      int Nhalf(std::floor( N/2));

      cout << "S2TMatrix epoch:"<< epoch << endl;
      
      epoch.setTimeSystem( TimeSystem::Unknown );

      if(QuaterMap.find(epoch)!=QuaterMap.end())
      {
          cout << "found!" << endl;
          double q0, q1, q2, q3;

          q0 = QuaterMap[epoch][0];
          q1 = QuaterMap[epoch][1];
          q2 = QuaterMap[epoch][2];
          q3 = QuaterMap[epoch][3];

          cout << "quaternion" << endl;
          cout << q0 << endl;
          cout << q1 << endl;
          cout << q2 << endl;
          cout << q3 << endl;

          rotMatrix = SRF2TRF(epoch, q0, q1, q2, q3);

          return rotMatrix;
          
      }

      cout << "don't find epoch in current QuaterMap" << endl;

      
      auto iter1 = QuaterMap.lower_bound(epoch);
      if(iter1 == QuaterMap.begin())
      {
          if( std::abs(epoch - iter1->first) > maxInterval )
          {
              cerr << "LEOAttitude doesn't match the input epoch!!" << endl;
              exit(-1);
          }
          else
          {
              double q0, q1, q2, q3;
              q0 = iter1->second[0];
              q1 = iter1->second[1];
              q2 = iter1->second[2];
              q3 = iter1->second[3];

              cout << "quaternion" << endl;
              cout << q0 << endl;
              cout << q1 << endl;
              cout << q2 << endl;
              cout << q3 << endl;

              rotMatrix = SRF2TRF(epoch, q0, q1, q2, q3);

              return rotMatrix;
          }
      }
      else
      {

         std::vector<double> times, Q0, Q1, Q2, Q3;

         // find the first element with key >= ttag
         auto iterStart = QuaterMap.lower_bound(epoch);
         for (int i=0; i<Nhalf; i++)
         {
            if(iterStart == QuaterMap.begin()) 
            {
               break;
            }
            --iterStart;
         }

         CommonTime firstEpoch = iterStart->first;
         // now, let's store N elements starting with the iterStart;
         for( int i=0; i<N; i++)
         {
            if( iterStart != QuaterMap.end() )
            {
               times.push_back(iterStart->first - firstEpoch);
               Q0.push_back(iterStart->second[0]);
               Q1.push_back(iterStart->second[1]);
               Q2.push_back(iterStart->second[2]);
               Q3.push_back(iterStart->second[3]);
               ++iterStart;
            }
            else
            {
               break;
            }
         }
         
         double dt(epoch - firstEpoch), err;

         cout << "times" << endl;
         for(auto e:times)
         {
             cout << e << endl;
         }

         cout << "Q0" << endl;
         for(auto e:Q0)
         {
             cout << e << endl;
         }
       
         double q0Int, q1Int, q2Int, q3Int;
         q0Int = LagrangeInterpolation(times, Q0, dt, err);
         q1Int = LagrangeInterpolation(times, Q1, dt, err);
         q2Int = LagrangeInterpolation(times, Q2, dt, err);
         q3Int = LagrangeInterpolation(times, Q3, dt, err);

         rotMatrix = SRF2TRF(epoch, q0Int, q1Int, q2Int, q3Int);
       
         times.clear();
         Q0.clear();
         Q1.clear();
         Q2.clear();
         Q3.clear();
         
         return rotMatrix;
      }

   }

   Matrix<double> LEOAttitude::T2SMatrix(CommonTime& epoch)
   {
         // The corresponding second in N half.
      Matrix<double> rotMatrix;

      int N(9);
      int Nhalf(std::floor( N/2));
      

      auto iter1 = QuaterMap.lower_bound(epoch);
      if(iter1 == QuaterMap.begin())
      {
          if( std::abs(epoch - iter1->first) > maxInterval )
          {
              cerr << "LEOAttitude doesn't match the input epoch!!" << endl;
              exit(-1);
          }
          else
          {
              double q0, q1, q2, q3;
              q0 = iter1->second[0];
              q1 = iter1->second[1];
              q2 = iter1->second[2];
              q3 = iter1->second[3];

              rotMatrix = SRF2TRF(epoch, q0, q1, q2, q3);

              return rotMatrix;
          }
      }
      else
      {

         std::vector<double> times, Q0, Q1, Q2, Q3;

         // find the first element with key >= ttag
         auto iterStart = QuaterMap.lower_bound(epoch);
         for (int i=0; i<Nhalf; i++)
         {
            if(iterStart == QuaterMap.begin()) 
            {
               break;
            }
            --iterStart;
         }

         CommonTime firstEpoch = iterStart->first;
         // now, let's store N elements starting with the iterStart;
         for( int i=0; i<N; i++)
         {
            if( iterStart != QuaterMap.end() )
            {
               times.push_back(iterStart->first - firstEpoch);
               Q0.push_back(iterStart->second[0]);
               Q1.push_back(iterStart->second[1]);
               Q2.push_back(iterStart->second[2]);
               Q3.push_back(iterStart->second[3]);
               ++iterStart;
            }
            else
            {
               break;
            }
         }
         
         double dt(epoch - firstEpoch), err;
       
         double q0Int, q1Int, q2Int, q3Int;
         q0Int = LagrangeInterpolation(times, Q0, dt, err);
         q1Int = LagrangeInterpolation(times, Q1, dt, err);
         q2Int = LagrangeInterpolation(times, Q2, dt, err);
         q3Int = LagrangeInterpolation(times, Q3, dt, err);
       
         rotMatrix = TRF2SRF(epoch, q0Int, q1Int, q2Int, q3Int);
       
         times.clear();
         Q0.clear();
         Q1.clear();
         Q2.clear();
         Q3.clear();
         
         return rotMatrix;
      }

   }


   Triple LEOAttitude::OffsetInSRF2TRF( CommonTime& epoch,
                                        Triple& offsetInSRF)
   {

         // convert Triple to Vector for Matrix computation.
      Vector<double> offsetInSRFVec(3);
      for(int i=0; i<3; i++)
      {
         offsetInSRFVec(i) = offsetInSRF[i];
      }
       
         // Vector and Triple to store the result.
      Vector<double> offsetInTRFVec(3);
      Triple offsetInTRF;

      cout << offsetInTRFVec << endl;

      cout << "OffsetInTRF2SRF" << endl;
      try
      {
         offsetInTRFVec = S2TMatrix(epoch) * offsetInSRFVec;
      }
      catch(...)
      {
         std::cerr << "Matrix Computation error..." << std::endl;
         exit(-1);
      }

      for(int i=0; i<3; i++)
      {
         offsetInTRF[i] = offsetInTRFVec(i);
      }
     
//    std::cout << "offset in SRF: " << offsetInSRF << std::endl;
//    std::cout << "offset in TRF: " << offsetInTRF << std::endl;

      return offsetInTRF;
   }
   

   Triple LEOAttitude::OffsetInTRF2SRF(CommonTime& epoch, 
                                       Triple& offsetInTRF)
   {

         // convert Triple to Vector for Matrix computation.
      Vector<double> offsetInTRFVec(3);
      for(int i=0; i<3; i++)
      {
         offsetInTRFVec(i) = offsetInTRF[i];
      }
      
         // Vector and Triple to store the result.
      Vector<double> offsetInSRFVec(3);
      Triple offsetInSRF;

      
      try
      {
         offsetInSRFVec = T2SMatrix(epoch) * offsetInTRFVec;
      }
      catch(...)
      {
         std::cerr << "Matrix Computation error..." << std::endl;
         exit(-1);
      }

      for(int i=0; i<3; i++)
      {
         offsetInSRF[i] = offsetInSRFVec(i);
      }
     
//    std::cout << "offset in SRF: " << offsetInSRF << std::endl;
//    std::cout << "offset in TRF: " << offsetInTRF << std::endl;

      return offsetInSRF;
   }

   
   Matrix<double> LEOAttitude::SRF2TRF(CommonTime epoch,
                                       double q0,
                                       double q1,
                                       double q2,
                                       double q3)
   {
      Matrix<double> CRF2TRF(3,3,0.0);
      Matrix<double> RotMatrix(3,3,0.0);

         // Secondly, calculate Rotation Matrix from CRF(Celestial RF)
         // to TRF(Terrestial RF)
      if(pRefSystem==NULL)
      {
         std::cerr << "Please first set reference system first.." << std::endl;
         exit(-1);
      }

      cout << "epoch:" << epoch << endl;

      CommonTime UTC( (*pRefSystem).GPS2UTC(epoch) );

      if(debug)
      cout << "UTC:" << UTC << endl;

      try
      {
         CRF2TRF = (*pRefSystem).C2TMatrix(UTC);
      }
      catch(...)
      {
         std::cerr << "Unable to get C2TMatrix from ReferenceSystem" 
                   << std::endl;
         exit(-1);
      }

      cout <<"CRF2TRF"<< endl; 
      cout << CRF2TRF << endl; 
      
      try
      {
         RotMatrix = CRF2TRF * SRF2CRF(q0, q1, q2, q3);

         cout << "SRF2CRF" << endl;
         cout <<  SRF2CRF(q0, q1, q2, q3)  << endl;
      }
      catch(...)
      {
         std::cerr << "Matrix Computation Error2 ..." << std::endl;
      }
      
      return RotMatrix;

   } // END of "LEOAttitude::quaternionToRotMatrix()"

    

      // First of all, calculate Rotation Matrix from SRF to IRF (Interial
      // Reference Frame) by quaternion.
      // Reference: Textbook.
      // Mention: The rotation matrix is a left one with respect to:
      // ----- CurrX * R_left = ObjX -----
      // ----- R_right * CurrX = ObjX ----
      // and the relationship between the left rotation matrix and the
      // right one is: --- R_left = transpose(R_right) or inverse(R_right)
    
      // X_SRF * SRF2CRF_L = X_CRF 
      // SRF2CRF_R * X_SRF = X_CRF
      // SRF2CRF_R = transpose(SRF2CRF_L)
      // Here, the result of  LEOAttitude::SRF2CRF is the right one.
   Matrix<double> LEOAttitude::SRF2CRF(double q0, 
                                       double q1, 
                                       double q2, 
                                       double q3)
   {
      Matrix<double> SRF2CRF_L(3,3,0.0);
      Matrix<double> SRF2CRF_R(3,3,0.0);

      SRF2CRF_L(0,0) = std::pow(q0,2) + std::pow(q1,2) - std::pow(q2,2) - std::pow(q3, 2);

      SRF2CRF_L(0,1) = 2.0* (q1*q2 + q0*q3);
      SRF2CRF_L(0,2) = 2.0* (q1*q3 - q0*q2);

      SRF2CRF_L(1,0) = 2.0* (q1*q2 - q0*q3);
      SRF2CRF_L(1,1) = std::pow(q0,2) - std::pow(q1,2) + std::pow(q2,2) - std::pow(q3, 2);
      SRF2CRF_L(1,2) = 2.0* (q2*q3 + q0*q1);

      SRF2CRF_L(2,0) = 2.0* (q1*q3 + q0*q2);
      SRF2CRF_L(2,1) = 2.0* (q2*q3 - q0*q1);
      SRF2CRF_L(2,2) = std::pow(q0,2) - std::pow(q1,2) - std::pow(q2,2) + std::pow(q3, 2);

      SRF2CRF_R = transpose(SRF2CRF_L);

      return SRF2CRF_R;

   }


      // IRF2SRF = transpose(SRF2CRF)
   Matrix<double> LEOAttitude::CRF2SRF(double q0,       
                                    double q1, 
                                    double q2, 
                                    double q3)
   {                                   
      return transpose( SRF2CRF(q0,q1,q2,q3) );
   }
   



      // Usage: X_SRF = TRF2SRF * X_TRF
   Matrix<double> LEOAttitude::TRF2SRF(CommonTime epoch,
                                    double q0,
                                    double q1,
                                    double q2,
                                    double q3)
   {

      Matrix<double> T2CMatrix(3,3,0.0);
      Matrix<double> C2SMatrix(3,3,0.0);
      Matrix<double> RotMatrix(3,3,0.0);

      if(pRefSystem==NULL)
      {
          cerr << "ReferenceSystem must be given first" << endl;
          exit(-1);
      }

      CommonTime UTC( (*pRefSystem).GPS2UTC(epoch) );

      T2CMatrix = (*pRefSystem).T2CMatrix(UTC);

      C2SMatrix = CRF2SRF(q0, q1, q2, q3);

      RotMatrix = C2SMatrix * T2CMatrix;

      return RotMatrix;

   }



} // END of "namespace gpstk"
